#include "gyro_algorithm.h"
#include "../Components/ATK_MS901M/atk_ms901m.h"
#include <math.h>

#define GYRO_STADY_MIN_ERR (3.0f)

#define TAG "Gyro_algo"

extern atk_ms901m_attitude_data_t attitude_dat;
float target_attitude = 0;

void gyro_set_target_turn_angle(float turn_angle)
{
    atk_ms901m_get_attitude(&attitude_dat, 10000);
    target_attitude = attitude_dat.yaw + turn_angle;
}

gyro_turn_state_t gyro_get_turn_err(float *_err)
{
    atk_ms901m_get_attitude(&attitude_dat, 100);
    float err = attitude_dat.yaw - target_attitude;

    int8_t flag = -1;
    if (err > 180.0)
    {
        err = 360.0 - err;
        flag = 1;
    }
    else if (err < -180.0)
    {
        err = -360.0 - err;
        flag = 1;
    }
    if (_err != NULL)
        *_err = (err);

    // log(TAG, "gyro_err: %.2f", err);

    if (fabs(err) < GYRO_STADY_MIN_ERR)
    {
        return GYRO_TURN_DONE;
    }
    else
    {
        return GYRO_TURN_ING;
    }
}